/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 *
 * $Id: CoordinateDriveServer.cpp,v 1.15 2009-10-22 12:50:22 morsko Exp $
 *
 * \file
 * \brief MaCI - Implementation of CoordinateDrive Server interface
 * \author Matthieu Myrsky <matthieu.myrsky@tkk.fi>
 */
#include "CoordinateDriveServer.hpp"
//*****************************************************************************
#define GIMI_CLIENT_API (20000)
#include "gimi.h"
//*****************************************************************************
//*****************************************************************************
using namespace MaCI::CoordinateDrive;
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************


CCoordinateDriveServer::CCoordinateDriveServer(MaCICtrl::CMaCICtrlServer *aMaCICtrlServerPtr, const int aInterfaceMinor)
  : CMaCI(aMaCICtrlServerPtr, GIMI_PROTOCOL_MACI_COORDINATEDRIVE, aInterfaceMinor),
    CThread(1),
    CSync(1,1),
    iServerActive(false),
    iCoordinateDriveCommandArray(),
    iReplyDataArray()
{


}
//*****************************************************************************

CCoordinateDriveServer::~CCoordinateDriveServer()
{
  Close();
}
//*****************************************************************************

MaCI::EMaCIError CCoordinateDriveServer::DoOpen(void)
{
  EMaCIError result = KMaCIError;
  assert(this != NULL);
  assert(iGIMIPtr != NULL);

  if(!iServerActive){
    RegisterProvidedService("MaCI - CoordinateDrive Interface");
    RegisterAcceptedService("MaCI - CoordinateDrive CommandInterface");
    iGIMIPtr->setQueueMaxSize(GIMI_PROTOCOL_MACI_COORDINATEDRIVE,iInterfaceMinor,1000,iRedirectionBufferId);
    iServerActive = true;
    SetActive(true);
    CThread::RunThread();
    result = KMaCIOK;
  }else{
    dPrint(3, "Server already opened");

  }
  return result;
}
//*****************************************************************************

MaCI::EMaCIError CCoordinateDriveServer::DoClose(void)
{
 EMaCIError result = KMaCIError;
 assert(this != NULL);
 assert(iGIMIPtr != NULL);
 if (iServerActive) {
   // Unregister services
    UnregisterProvidedService();
    UnregisterAcceptedService();
   // Stop the handler Thread
   iServerActive = false;
   SetActive(false);
   ///* CThread::CancelThread(); //Removed by script */
   CThread::WaitThread();
   
 }
 return result;
}
//*****************************************************************************

MaCI::EMaCIError CCoordinateDriveServer::WaitCommand(CCoordinateDriveData &aData, 
						     TReplyInfo &aInfo,
						     const unsigned int aTimeout_ms,
						     bool aSkipQueued)
{
  EMaCIError result = KMaCIError;
  const unsigned int begin = ownTime_get_ms();
  int timeout = aTimeout_ms;
  
  aData.Reset();
  
  Lock();
  do {
    const unsigned int asize = iCoordinateDriveCommandArray.size();
    // Check whether there are commands in buffer.
    if (asize > 0) {
      // Got element. So; take reference of its pointer, copy it to
      // 'data', delete it. Pop the element from queue.
      const CCoordinateDriveData *d;
      const TReplyInfo *r;
      
      // Check whether we want to skip queue 
      if (aSkipQueued && asize > 1) {
        // This is only executed if there is >1 in queue.  Otherwise
        // default branch is executed.  Want to skip, so get back()
        // reference.
        d = iCoordinateDriveCommandArray.back();
	r= iReplyDataArray.back();
        // Copy data
        aData = *d;
	aInfo = *r;

        // Delete all elements.
        for(EACH_IN_i(iCoordinateDriveCommandArray)) {
          delete *i;
        }
        for(EACH_IN_i(iReplyDataArray)) {
          delete *i;
        }
        // Clear array.
        iCoordinateDriveCommandArray.clear();
	iReplyDataArray.clear();

        // print debug
        dPrint(4,"Flushed %d unprocessed commands out of buffer",
               asize-1);

      } else {
        // No skipping, so get front() reference;
        d = iCoordinateDriveCommandArray.front();
	r = iReplyDataArray.front();
        // Copy data.
        aData = *d;
	aInfo = *r;
        // Delete source
        delete d;
	delete r;
        // pop the entry from queue.
        iCoordinateDriveCommandArray.pop_front();
	iReplyDataArray.pop_front();

      }

      // Set result.
      result = KMaCIOK;
      
      // Break the loop, skipping Wait()
      break;
    }
    
    // Wait signal unconditionally.
    if (timeout > 0) Wait(timeout);
    
    // Recalculate timeout
    timeout = aTimeout_ms - ownTime_get_ms_since(begin);
  } while(timeout > 0 && iServerActive);
  Unlock();

  // Change result type if timeout was the reason.
  if (timeout <= 0 && result == KMaCIError) result = KMaCITimeout;


  return result;



}
//*****************************************************************************

bool CCoordinateDriveServer::SendArrivedToTargetEvent(CCoordinateDriveData &aData)
{
  bool result = false;
  assert(iGIMIPtr != NULL);
  if(aData.IsValid()){
    const TCoordinate2D *c = aData.GetCoordinate(0);
    if (c) {

      if(aData.GetTimestamp() == NULL){
        aData.SetTimestamp(MaCI::Common::TTimestamp());
      }
      dPrint(15,"Sending event");
      EMaCIError e = 
        SendInterfaceSubscriberMessage(aData.GetBinBagContainerPtr()->GetBuffer(),
                                       aData.GetBinBagContainerPtr()->GetBufferSize());
      result = (e == KMaCIOK);
      
    } else {
      dPrint(4,"No TCoordinate2D element in container, will not send!");
    }
  }else{
    dPrint(3,"CoordinateDrive isn't valid, will not send event!");
  }

  return result;

}
//*****************************************************************************

bool CCoordinateDriveServer::SendCoordinatesEvent(CCoordinateDriveData &aData)
{
  bool result = false;
  assert(iGIMIPtr != NULL);
  if(aData.IsValid()){
    const TCoordinateHeader * h = aData.GetCoordinateHeader();
    

    if (h) {
      dPrint(15,"Sending event");
      EMaCIError e = 
        SendInterfaceSubscriberMessage(aData.GetBinBagContainerPtr()->GetBuffer(),
                                       aData.GetBinBagContainerPtr()->GetBufferSize());
      if(e != KMaCIOK){
        dPrint(1,"Failed to send coordinates event (%s)",GetErrorStr(e).c_str());
      }
      result = (e == KMaCIOK);
   
    } else {
      if(h == NULL){
        dPrint(4,"No TCoordinateHeader element in container, can't send event without it!");
      }
    }
  }else{
    dPrint(3,"CoordinateDrive isn't valid, can't send event!");
  }

  return result;




}
//*****************************************************************************

bool CCoordinateDriveServer::UnableToDriveToCoordinate(TCoordinate2D &aCoordinate)
{

  assert(iGIMIPtr != NULL);
  CCoordinateDriveData data;
  data.CreateInternalBinBag();
  data.SetCommand(KCommandUnableToDriveToCoordinate);
  data.SetCoordinate2D(aCoordinate);
  dPrint(15,"Sending event");
  EMaCIError e = 
    SendInterfaceSubscriberMessage(data.GetBinBagContainerPtr()->GetBuffer(),
                                   data.GetBinBagContainerPtr()->GetBufferSize());
  if(e != KMaCIOK){
    dPrint(1,"Failed to send coordinates event (%s)",GetErrorStr(e).c_str());
  }
  return (e == KMaCIOK);

 
}
//*****************************************************************************

bool CCoordinateDriveServer::SendCalculatingEvent()
{

  assert(iGIMIPtr != NULL);
  CCoordinateDriveData data;
  data.CreateInternalBinBag();
  data.SetCommand(KCommandCalculating);
  dPrint(15,"Sending event");
  EMaCIError e = 
    SendInterfaceSubscriberMessage(data.GetBinBagContainerPtr()->GetBuffer(),
                                   data.GetBinBagContainerPtr()->GetBufferSize());
  if(e != KMaCIOK){
    dPrint(1,"Failed to send coordinates event (%s)",GetErrorStr(e).c_str());
  }
  return (e == KMaCIOK);

 
}
//*****************************************************************************

bool CCoordinateDriveServer::SendPathReadyEvent()
{

  assert(iGIMIPtr != NULL);
  CCoordinateDriveData data;
  data.CreateInternalBinBag();
  data.SetCommand(KCommandPathReady);
  dPrint(15,"Sending event");
  EMaCIError e = 
    SendInterfaceSubscriberMessage(data.GetBinBagContainerPtr()->GetBuffer(),
                                   data.GetBinBagContainerPtr()->GetBufferSize());
  if(e != KMaCIOK){
    dPrint(1,"Failed to send coordinates event (%s)",GetErrorStr(e).c_str());
  }
  return (e == KMaCIOK);

 
}
//*****************************************************************************

MaCI::EMaCIError CCoordinateDriveServer::SendReply(CCoordinateDriveData &aData,
						   TReplyInfo &aInfo)
{
  EMaCIError result = KMaCIError;
  
  if (iServerActive) {
    if (aData.IsValid()) {
      dPrint(ODDEBUG(1),"Sending Reply to %x, msgid: %u", aInfo.senderId, aInfo.msgId);
      result = SendSynchronousMessageReply(aData.GetBinBagContainerPtr()->GetBuffer(),
                                           aData.GetBinBagContainerPtr()->GetBufferSize(),
                                           aInfo.senderId,
                                           iInterfaceMajor,
                                           iInterfaceMinor,
                                           aInfo.msgId);

    }
  }
  
  return result;
}
//*****************************************************************************

int CCoordinateDriveServer::ThreadFunction(const int aThreadNumber)
{
  int result = 0;
  EMaCIError e;
  gimi::GIMIMessage gmsg;
  
  assert(aThreadNumber == 0);
  assert(iGIMIPtr != NULL);
  
  dPrint(4, "GIMIMessage handler started");
  
  // Run as long as the thread should be active
  while(iServerActive) {
    // Call the ReceiveMessage wrapper with 1s timeout
    e = ReceiveInterfaceMessage(gmsg, 1000);
    if (e == KMaCIOK) {
      dPrint(15,"Got GIMIMessage %d - Processing.(sender id 0x%08x)", gmsg.messageId,gmsg.senderId);
      ProcessMessage(gmsg);
      
    } else if (e != KMaCITimeout) {
      // Other error, try to recover by sleeping one second.
      ownSleep_ms(1000);
      
    }
  }
  
  dPrint(4, "GIMIMessage handler stopped");
  
  return result;

}
//*****************************************************************************

int CCoordinateDriveServer::ProcessMessage(const gimi::GIMIMessage &msg)
{
 int result = 0;
  gim::binbag::CBinBag bb;
  CCoordinateDriveData d;
  bool r;

  r = bb.Decode(msg.getData(), msg.getDatalength());
  if (r) {
    // Succesfully decoded as BinBag.
    r = d.DecodeFrom(&bb);
    if (r) {
      // Succesfully decoded as SpeedCtrl
      // Now, Check whether it was Command, if so, process it 
   
      const TCommand *cmd = d.GetCommand();
      if (cmd) {
        //Check the type of Command, if it's an event msg it's not for the server
	if(cmd->cmd != KCommandUnknown &&
           cmd->cmd != KCommandArrivedToPointEvent &&
           cmd->cmd != KCommandCoordinatesEvent){
          // Default action for All commands is to save them to CommandQueue
          
          // Debug
          dPrint(8, "Got Command to be passed to User (Command was %d, msgId: %u)",
                 cmd->cmd, msg.messageId);
	  CCoordinateDriveData *new_entry = new CCoordinateDriveData(d);
	  TReplyInfo *new_reply = new TReplyInfo(msg.getSenderId(),msg.messageId);
          // Lock, Push, Notify, Unlock
          Lock();
          iCoordinateDriveCommandArray.push_back(new_entry);
	  iReplyDataArray.push_back(new_reply);
          Notify();
          Unlock();
        
        }else{  

          dPrint(4,"Unidentified command! %d (%x)! Ignore.",
                 cmd->cmd, cmd->cmd);
        
        }
      }
      
    }
  }
  
  return result;
}
//*****************************************************************************

void CCoordinateDriveServer::ClearCommandBuffer(void)
{
  Lock();
  dPrint(8,"Clearing commandbuffer, got %d entries", iCoordinateDriveCommandArray.size());
  iCoordinateDriveCommandArray.clear();
  Unlock();
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************




